#include "bsp_can.h"
#include "stdio.h"
#include "DJI_motor.h"

#include "ARM.h"

uint8_t buf[8];
FDCAN_RxHeaderTypeDef fdcan_RxHeader1;
// FDCAN_RxHeaderTypeDef fdcan_RxHeader2;
uint32_t id;
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    if ((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != RESET)
    {
        HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &fdcan_RxHeader1, buf);
        id = fdcan_RxHeader1.Identifier - 0x201;
        if (id >= 0 && id <= 4)
        {
            if (hfdcan == &hfdcan1) // can1 用于claw
            {
                Claw_Motor[id].last_angle = Claw_Motor[id].angle;
                Claw_Motor[id].angle = (uint16_t)(buf[0] << 8 | buf[1]);
                Claw_Motor[id].rpm = (uint16_t)(buf[2] << 8 | buf[3]);
                Claw_Motor[id].current = (uint16_t)(buf[4] << 8 | buf[5]);
                Claw_Motor[id].current = (uint16_t)(buf[4] << 8 | buf[5]);
                if (Claw_Motor[id].angle - Claw_Motor[id].last_angle > 4096)
                    Claw_Motor[id].round--;
                else if (Claw_Motor[id].angle - Claw_Motor[id].last_angle < -4096)
                    Claw_Motor[id].round++;
                Claw_Motor[id].totall_angle = (Claw_Motor[id].angle + Claw_Motor[id].round * 8192 - Claw_Motor[id].offset_angle)/100;
            }
            else if (hfdcan == &hfdcan2) // can2 用于arm
            {
                Arm_Motor[id].last_angle = Arm_Motor[id].angle;
                Arm_Motor[id].angle = (uint16_t)(buf[0] << 8 | buf[1]);
                Arm_Motor[id].rpm = (uint16_t)(buf[2] << 8 | buf[3]);
                Arm_Motor[id].current = (uint16_t)(buf[4] << 8 | buf[5]);
                Arm_Motor[id].current = (uint16_t)(buf[4] << 8 | buf[5]);
                if (Arm_Motor[id].angle - Arm_Motor[id].last_angle > 4096)
                    Arm_Motor[id].round--;
                else if (Arm_Motor[id].angle - Arm_Motor[id].last_angle < -4096)
                    Arm_Motor[id].round++;
                Arm_Motor[id].totall_angle = (Arm_Motor[id].angle + Arm_Motor[id].round * 8192 - Arm_Motor[id].offset_angle)/100;
            }
            else if (hfdcan == &hfdcan3) // can3 用于chassis
            {
                if (id < 2)
                {
                    Chassis_Motor[id].last_angle = Chassis_Motor[id].angle;
                    Chassis_Motor[id].angle = (uint16_t)(buf[0] << 8 | buf[1]);
                    Chassis_Motor[id].rpm = (uint16_t)(buf[2] << 8 | buf[3]);
                    Chassis_Motor[id].current = (uint16_t)(buf[4] << 8 | buf[5]);
                    Chassis_Motor[id].current = (uint16_t)(buf[4] << 8 | buf[5]);
//									if(id==2)
//									{
//										if (Claw_Motor[4].angle - Claw_Motor[4].last_angle > 4096)
//                        Claw_Motor[4].round--;
//                    else if (Claw_Motor[4].angle - Claw_Motor[4].last_angle < -4096)
//                        Claw_Motor[4].round++;
//                    Claw_Motor[4].totall_angle = (Claw_Motor[4].angle + Claw_Motor[4].round * 8192 - Claw_Motor[4].offset_angle)/100;
//									}
                    if (Chassis_Motor[id].angle - Chassis_Motor[id].last_angle > 4096)
                        Chassis_Motor[id].round--;
                    else if (Chassis_Motor[id].angle - Chassis_Motor[id].last_angle < -4096)
                        Chassis_Motor[id].round++;
                    Chassis_Motor[id].totall_angle = (Chassis_Motor[id].angle + Chassis_Motor[id].round * 8192 - Chassis_Motor[id].offset_angle)/100;
                }
								else if(id==2)
								{
									uint8_t i;
										RxData[0]=buf[0];
								
								}
            }
        }
    }
}
